"""
#===============================================================================
# License
#===============================================================================
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

#===============================================================================
Copyright (c) 2009 bryan p. conrad
bconrad2@gmail.com

Repository Path:    
$HeadURL$

Created on:           Oct 20, 2009
Last Revised on:      $Date$
Version:              $Revision$
ID:                   $Id$
Author:               $Author$ :: (bryan)

#===============================================================================


#===============================================================================
# Documentation
#===============================================================================
    # Function for the conversion of one rotation input type to desired output.
    # Supported conversion input/output types are as follows:
    #    1: Q        Rotation Quaternions
    #    2: EV       Euler Vector and rotation angle (degrees)
    #    3: DCM      Orthogonal DCM Rotation Matrix
    #    4: EAXXX    Euler angles (12 possible sets) (degrees)
    # 
    # Author: John Fuller
    # National Institute of Aerospace
    # Hampton, VA 23666
    # John.Fuller@nianet.org
    # 
    # Version 1.3
    # June 30th, 2009
    # 
    # Version 1.3 updates
    #    SpinCalc now detects when input data is too close to Euler singularity, if user is choosing
    #    Euler angle output. Prohibits output if middle angle is within 0.1 degree of singularity value.
    # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    #                 OUTPUT=SpinCalc(CONVERSION,INPUT,tol,ichk)
    # Inputs:
    # CONVERSION - np.single string value that dictates the type of desired
    #              conversion.  The conversion strings are listed below.
    # 
    #    'DCMtoEAXXX'  'DCMtoEV'    'DCMtoQ'       **for cases that involve   
    #    'EAXXXtoDCM'  'EAXXXtoEV'  'EAXXXtoQ'       euler angles, XXX should be
    #    'EVtoDCM'     'EVtoEAXXX'  'EVtoQ'          replaced with the proper 
    #    'QtoDCM'      'QtoEAXXX'   'QtoEV'          order desired.  EA321 would
    #    'EAXXXtoEAXXX'                              be Z(yaw)-Y(pitch)-X(roll).
    # 
    # INPUT - matrix or vector that corresponds to the first entry in the
    #         CONVERSION string, formatted as follows:
    # 
    #         DCM - 3x3xN multidimensional matrix which pre-multiplies by a coordinate
    #               frame vector to rotate it to the desired new frame.
    # 
    #         EAXXX - [psi,theta,phi] (Nx3) row vector list dictating to the first angle
    #                 rotation (psi), the second (theta), and third (phi) (DEGREES)
    # 
    #         EV - [m1,m2,m3,MU] (Nx4) row vector list dictating the components of euler
    #              rotation vector (original coordinate frame) and the Euler 
    #              rotation angle about that vector (MU) (DEGREES)
    # 
    #         Q - [q1,q2,q3,q4] (Nx4) row vector list defining quaternion of
    #             rotation.  q4 = np.cos(MU/2) where MU is Euler rotation angle
    # 
    # tol - tolerance value
    # ichk - 0 disables warning flags
    #           1 enables warning flags (near np.singularities)
    # **NOTE: N corresponds to multiple orientations
    # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    # Output:
    # OUTPUT - matrix or vector corresponding to the second entry in the
    #          CONVERSION input string, formatted as shown above.
    # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
"""

#===============================================================================
# Imports
#===============================================================================
import numpy as np
from pylab import find

def SpinCalc(CONVERSION,INPUT,units='rad',tol=1e-6,ichk=1):
    
    INPUT = np.asarray(INPUT)
    
    # Pre-processer to determine type of conversion from CONVERSION string input
    # Types are numbered as follows:
    # Q=1   EV=2   DCM=3   EA=4
    i_type,o_type = CONVERSION.lower().split('to')
    length=len(CONVERSION)
    if length>12 or length<4:   # no CONVERSION string can be shorter than 4 or longer than 12 chars
        print ('Exception: Invalid entry for CONVERSION input string')

    if len(i_type)<5:
        i_type=len(i_type)
    else:
        i_type=len(i_type)-1

    if len(o_type)<5:
        o_type=len(o_type)
    else:
        o_type=len(o_type)-1

    TYPES={}
    TYPES[1]='Q' 
    TYPES[2]='EV' 
    TYPES[3]='DCM' 
    TYPES[4]='EA'
#    TYPES['Q'] = 1 
#    TYPES['EV'] = 2 
#    TYPES['DCM'] = 3 
#    TYPES['EA'] = 4
    INPUT_TYPE=TYPES[i_type]
    OUTPUT_TYPE=TYPES[o_type]

    # Confirm input as compared to program interpretation
    if i_type!=4 and o_type!=4:  # if input/output are NOT Euler angles
        CC=INPUT_TYPE+'to'+OUTPUT_TYPE
        if CONVERSION != CC:
            print ('Exception: Invalid entry for CONVERSION input string')        
        
    else:
        if i_type==4:   # if input type is Euler angles, determine the order of rotations
            if units=='deg':
                # if input is in degrees convert it to radians, will be 
                # converted back to degrees for output.
                INPUT = INPUT*np.pi/180
            EULER_order_in=int(CONVERSION[2:5])
            rot_1_in=int(np.floor(EULER_order_in/100))     # first rotation
            rot_2_in=int(np.floor((EULER_order_in-rot_1_in*100)/10))   # second rotation
            rot_3_in=int(EULER_order_in-rot_1_in*100-rot_2_in*10)   # third rotation
            if rot_1_in<1 or rot_2_in<1 or rot_3_in<1 or rot_1_in>3 or rot_2_in>3 or rot_3_in>3:
                print ('Exception: Invalid input Euler angle order type (conversion string).')  # check that all orders are between 1 and 3            
            elif rot_1_in==rot_2_in or rot_2_in==rot_3_in:
                print ('Exception: Invalid input Euler angle order type (conversion string).')  # check that no 2 consecutive orders are equal (invalid)           
            
            # check input dimensions to be 1x3x1
            if INPUT.shape[0]!=3: # or INPUT.shape[2]!=1:
                print ('Exception: Input euler angle data vector is not Nx3')            
            
            # identify np.singularities        
            if rot_1_in==rot_3_in: # Type 2 rotation (first and third rotations about same axis)
                if (INPUT[1]<=0) or (INPUT[1]>=np.pi):  # confirm second angle within range
                    print ('Exception: Second input Euler angle(s) outside 0 to 180 degree range')
                elif (np.abs(INPUT[1])<2) or np.abs((INPUT[1])>(np.pi-0.035)):  # check for np.singularity 88 deg
                    if ichk==1:
                        print ('Warning: Input Euler angle rotation(s) near a singularity.               Second angle near 0 or 180 degrees.')
                    
                
            else:    # Type 1 rotation (all rotations about each of three axes)
                if np.abs(INPUT[1])>=np.pi/2: # confirm second angle within range
                    print ('Exception: Second input Euler angle(s) outside -90 to 90 degree range')
                elif np.abs(INPUT[1])>(np.pi/2-0.035):  # check for np.singularity
                    if ichk==1:
                        print ('Warning: Input Euler angle(s) rotation near a singularity.               Second angle near -90 or 90 degrees.')
                
        
        if o_type==4:   # if output type is Euler angles, determine order of rotations
            EULER_order_out=int(CONVERSION[-3:])
            rot_1_out=int(np.floor(EULER_order_out/100))   # first rotation
            rot_2_out=int(np.floor((EULER_order_out-rot_1_out*100)/10))    # second rotation
            rot_3_out=int(EULER_order_out-rot_1_out*100-rot_2_out*10) # third rotation
            if rot_1_out<1 or rot_2_out<1 or rot_3_out<1 or rot_1_out>3 or rot_2_out>3 or rot_3_out>3:
                print ('Exception: Invalid output Euler angle order type (conversion string).') # check that all orders are between 1 and 3           
            elif rot_1_out==rot_2_out or rot_2_out==rot_3_out:
                print ('Exception: Invalid output Euler angle order type (conversion string).') # check that no 2 consecutive orders are equal
            
                
        if i_type==4 and o_type!=4:  # if input are euler angles but not output
            CC='EA'+str(EULER_order_in)+'to'+OUTPUT_TYPE # construct program conversion string for checking against user input
        elif o_type==4 and i_type!=4:  # if output are euler angles but not input
            CC=INPUT_TYPE+'to'+'EA'+str(EULER_order_out) # construct program conversion string for checking against user input
        elif i_type==4 and o_type==4:  # if both input and output are euler angles
            CC='EA'+str(EULER_order_in)+'to'+'EA'+str(EULER_order_out)   # construct program conversion string
        elif i_type!=4 and o_type==4:  # if both input and output are euler angles
            CC=INPUT_TYPE+'to'+'EA'+str(EULER_order_out)
            
        if CONVERSION!=CC: # check program conversion string against user input to confirm the conversion command
            print ('Exception: Invalid entry for CONVERSION input string \n\t(given:%s, interpreted:%s)' % (CONVERSION, CC))

    # From the input, determine the quaternions that uniquely describe the
    # rotation prescribed by that input.  The output will be calculated in the
    # second portion of the code from these quaternions.
    if INPUT_TYPE == 'DCM':
        if (INPUT.shape!=(3,3)):
            if (INPUT.shape==(4,4)):  # check DCM dimensions
                INPUT=INPUT[0:3,0:3]
            else:
                print ('Exception: DCM matrix is not 3x3 or 4x4, it is: %s' % str(INPUT.shape))
                return           
        
        N=1    # number of orientations
        # Check if matrix is indeed orthogonal
        perturbed=np.ones((3,3))*np.nan
        DCM_flag=0
        for ii in range(N):
            perturbed[:,:]=(np.abs(np.dot(INPUT,INPUT.transpose())-np.eye(3))) # perturbed array shows difference between DCM*DCM' and I
            if np.abs(np.linalg.det(INPUT)-1)>tol: # if determinant is off by one more than tol, user is warned.
                if ichk==1:
                    DCM_flag=1
                
            
            if np.abs(np.linalg.det(INPUT)+1)<0.05: # if determinant is near -1, DCM is improper
                print ('Exception: Input DCM(s) improper')           
            
            if DCM_flag==1:
                print ('Warning: Input DCM matrix determinant(s) off from 1 by more than tolerance.')
                #print "np.abs(np.linalg.det(INPUT)-1) = ", np.abs(np.linalg.det(INPUT)-1)
                #print "Setting to I"
                #INPUT = np.eye(3)
        
        DCM_flag=0
        if ichk==1:
            for kk in range(N):
                for ii in range(3):
                    for jj in range(3):
                        if perturbed[ii,jj]>tol:   # if any difference is larger than tol, user is warned.
                            DCM_flag=1
                        
                    
            
            if DCM_flag==1:
                print 'Warning: Input DCM(s) matrix not orthogonal to precision tolerance.'
                #print "(np.abs(np.dot(INPUT,INPUT.transpose())-np.eye(3)))",(np.abs(np.dot(INPUT,INPUT.transpose())-np.eye(3))).max()
                #print "Setting to I"
                #INPUT = np.eye(3)
                  
        Q=np.ones((4))*np.nan
        for ii in range(N):
            denom=np.ones((4))*np.nan
            denom[0]=0.5*np.sqrt(np.abs(1+INPUT[0,0]-INPUT[1,1]-INPUT[2,2]))
            denom[1]=0.5*np.sqrt(np.abs(1-INPUT[0,0]+INPUT[1,1]-INPUT[2,2]))
            denom[2]=0.5*np.sqrt(np.abs(1-INPUT[0,0]-INPUT[1,1]+INPUT[2,2]))
            denom[3]=0.5*np.sqrt(np.abs(1+INPUT[0,0]+INPUT[1,1]+INPUT[2,2]))        
            # determine which Q equations maximize denominator
            #print "This section is not implemented"
            #raise
            case = find(denom==denom.max())[0]  # determines max value of qtests to put in denominator
                
            if case == 1:
                Q[0]=denom[0]
                Q[1]=(INPUT[0,1]+INPUT[1,0])/(4*Q[0])
                Q[2]=(INPUT[0,2]+INPUT[2,0])/(4*Q[0])
                Q[3]=(INPUT[1,2]-INPUT[2,1])/(4*Q[0])
            elif case == 2:
                Q[1]=denom[1]
                Q[0]=(INPUT[0,1]+INPUT[1,0])/(4*Q[1])
                Q[2]=(INPUT[1,2]+INPUT[2,1])/(4*Q[1])
                Q[3]=(INPUT[2,0]-INPUT[0,2])/(4*Q[1])
            elif case == 3:
                Q[2]=denom[2]
                Q[0]=(INPUT[0,2]+INPUT[2,0])/(4*Q[2])
                Q[1]=(INPUT[1,2]+INPUT[2,1])/(4*Q[2])
                Q[3]=(INPUT[0,1]-INPUT[1,0])/(4*Q[2])
            elif case == 4:
                Q[3]=denom[3]
                Q[0]=(INPUT[1,2]-INPUT[2,1])/(4*Q[3])
                Q[1]=(INPUT[2,0]-INPUT[0,2])/(4*Q[3])
                Q[2]=(INPUT[0,1]-INPUT[1,0])/(4*Q[3])
        
        
        Q=Q.transpose()

    elif INPUT_TYPE == 'EV':  # Euler Vector Input Type
        if INPUT.shape[0]!=4: # or INPUT.shape[2]!=1:   # check dimensions
            print ('Exception: Input euler vector and rotation data matrix is not Nx4')            
        
        N=INPUT.shape[0]
        if units=='deg':
            MU=INPUT[3]*np.pi/180  # assign mu name for clarity
        else:
            MU=INPUT[3]
        if np.sqrt(INPUT[0]**2+INPUT[1]**2+INPUT[2]**2)-np.ones((N))>tol*np.ones(N):  # check that input m's constitute unit vector
            print ('Input euler vector(s) components do not constitute a unit vector')            
        
        if MU<0 or MU>2*np.pi: # check if rotation about euler vector is between 0 and 360
            print ('Input euler rotation angle(s) not between 0 and 360 degrees')
        
        Q=np.asarray([INPUT[0]*np.sin(MU/2),INPUT[1]*np.sin(MU/2),INPUT[2]*np.sin(MU/2),np.cos(MU/2)])   # quaternion
     
    elif INPUT_TYPE == 'EA':   
        if units=='deg':  
            # convert from deg to radians if necessary   
            psi=INPUT[0]*np.pi/180  
            theta=INPUT[1]*np.pi/180  
            phi=INPUT[2]*np.pi/180
        else:
            psi=INPUT[0]  
            theta=INPUT[1]  
            phi=INPUT[2]
        N=INPUT.shape[0]    # number of orientations
        # Pre-calculate conp.sines and np.sines of the half-angles for conversion.
        c1=np.cos(psi/2) 
        c2=np.cos(theta/2) 
        c3=np.cos(phi/2)
        s1=np.sin(psi/2) 
        s2=np.sin(theta/2) 
        s3=np.sin(phi/2)
        c13=np.cos((psi+phi)/2)  
        s13=np.sin((psi+phi)/2)
        c1_3=np.cos((psi-phi)/2)  
        s1_3=np.sin((psi-phi)/2)
        c3_1=np.cos((phi-psi)/2)  
        s3_1=np.sin((phi-psi)/2)
        if EULER_order_in==121:
            Q=np.asarray([c2*s13, s2*c1_3, s2*s1_3, c2*c13])
        elif EULER_order_in==232:
            Q=np.asarray([s2*s1_3,c2*s13,s2*c1_3,c2*c13])
        elif EULER_order_in==313:
            Q=np.asarray([s2*c1_3,s2*s1_3,c2*s13,c2*c13])
        elif EULER_order_in==131:
            Q=np.asarray([c2*s13,s2*s3_1,s2*c3_1,c2*c13])
        elif EULER_order_in==212:
            Q=np.asarray([s2*c3_1,c2*s13,s2*s3_1,c2*c13])
        elif EULER_order_in==323:
            Q=np.asarray([s2*s3_1,s2*c3_1,c2*s13,c2*c13])
        elif EULER_order_in==123:
            Q=np.asarray([s1*c2*c3+c1*s2*s3,c1*s2*c3-s1*c2*s3,\
                          c1*c2*s3+s1*s2*c3,c1*c2*c3-s1*s2*s3])
        elif EULER_order_in==231:
            Q=np.asarray([c1*c2*s3+s1*s2*c3,s1*c2*c3+c1*s2*s3,
                          c1*s2*c3-s1*c2*s3,c1*c2*c3-s1*s2*s3])
        elif EULER_order_in==312:
            Q=np.asarray([c1*s2*c3-s1*c2*s3,c1*c2*s3+s1*s2*c3,
                          s1*c2*c3+c1*s2*s3,c1*c2*c3-s1*s2*s3])
        elif EULER_order_in==132:
            Q=np.asarray([s1*c2*c3-c1*s2*s3,c1*c2*s3-s1*s2*c3,
                          c1*s2*c3+s1*c2*s3,c1*c2*c3+s1*s2*s3])
        elif EULER_order_in==213:
            Q=np.asarray([c1*s2*c3+s1*c2*s3,s1*c2*c3-c1*s2*s3,
                          c1*c2*s3-s1*s2*c3,c1*c2*c3+s1*s2*s3])
        elif EULER_order_in==321:
            Q=np.asarray([c1*c2*s3-s1*s2*c3,c1*s2*c3+s1*c2*s3,
                          s1*c2*c3-c1*s2*s3,c1*c2*c3+s1*s2*s3])
        else:
            print ('Exception: Invalid input Euler angle order type (conversion string)')            
        
    elif INPUT_TYPE == 'Q':
        if INPUT.shape[0]!=4: # or INPUT.shape[2]!=1:
            print ('Exception: Input quaternion matrix is not Nx4')            
        
        N=INPUT.shape[0]    # number of orientations 
        if ichk==1:
            if np.abs(np.sqrt(INPUT[0]**2+INPUT[1]**2+INPUT[2]**2+INPUT[3]**2)-1)>tol*1:
                print ('Warning: Input quaternion norm(s) deviate(s) from unity by more than tolerance')
             
        
        Q=INPUT


    # Normalize quaternions in case of deviation from unity.  User has already
    # been warned of deviation.
    Qnorms=np.sqrt((Q*Q).sum())
    Q=Q/Qnorms  #np.asarray([Q[0]/Qnorms,Q[1]/Qnorms,Q[2]/Qnorms,Q[3]/Qnorms])

     
    if OUTPUT_TYPE == 'DCM':
        #Q=Q.transpose().reshape(1,4)
        OUTPUT=np.asarray([Q[0]**2-Q[1]**2-Q[2]**2+Q[3]**2,
                2*(Q[0]*Q[1]+Q[2]*Q[3]),
                2*(Q[0]*Q[2]-Q[1]*Q[3]),
                2*(Q[0]*Q[1]-Q[2]*Q[3]),
                -Q[0]**2+Q[1]**2-Q[2]**2+Q[3]**2,
                2*(Q[1]*Q[2]+Q[0]*Q[3]),
                2*(Q[0]*Q[2]+Q[1]*Q[3]),
                2*(Q[1]*Q[2]-Q[0]*Q[3]),
                -Q[0]**2-Q[1]**2+Q[2]**2+Q[3]**2])
        
        OUTPUT=OUTPUT.reshape((3,3))
    elif OUTPUT_TYPE == 'EV':
        MU=2*np.arctan2(np.sqrt((Q[0:2]*Q[0:2]).sum()),Q[3])
        if np.sin(MU/2)!=0:
            OUTPUT=np.asarray([Q[0]/np.sin(MU/2),
                    Q[1]/np.sin(MU/2),
                    Q[2]/np.sin(MU/2),
                    MU*180/np.pi])
        else:
            OUTPUT=np.ones((4))*np.nan
            for ii in range(N):
                if np.sin(MU[ii,0]/2)!=0:
                    if units=='deg':
                        OUTPUT[ii,0:3]=np.asarray([Q[ii,0]/np.sin(MU[ii,0]/2),
                                        Q[ii,1]/np.sin(MU[ii,0]/2),
                                        Q[ii,2]/np.sin(MU[ii,0]/2),
                                        MU[ii,0]*180/np.pi])
                    else:
                        OUTPUT[ii,0:3]=np.asarray([Q[ii,0]/np.sin(MU[ii,0]/2),
                                        Q[ii,1]/np.sin(MU[ii,0]/2),
                                        Q[ii,2]/np.sin(MU[ii,0]/2),
                                        MU[ii,0]])
                        
                else:
                    if units=='deg':
                        OUTPUT[ii,0:3]=[1,0,0,MU[ii,0]*180/np.pi]
                    else:
                        OUTPUT[ii,0:3]=[1,0,0,MU[ii,0]]
                
            
    elif OUTPUT_TYPE == 'Q':
        OUTPUT=Q
    elif OUTPUT_TYPE == 'EA':
        if EULER_order_out==121:
            psi=np.arctan2((Q[0]*Q[1]+Q[2]*Q[3]),(Q[1]*Q[3]-Q[0]*Q[2]))
            theta=np.arccos(Q[3]**2+Q[0]**2-Q[1]**2-Q[2]**2)
            phi=np.arctan2((Q[0]*Q[1]-Q[2]*Q[3]),(Q[0]*Q[2]+Q[1]*Q[3]))
            Euler_type=2
        elif EULER_order_out==232:
            psi=np.arctan2((Q[0]*Q[3]+Q[1]*Q[2]),(Q[2]*Q[3]-Q[0]*Q[1]))
            theta=np.arccos(Q[3]**2-Q[0]**2+Q[1]**2-Q[2]**2)
            phi=np.arctan2((Q[1]*Q[2]-Q[0]*Q[3]),(Q[0]*Q[1]+Q[2]*Q[3]))
            Euler_type=2
        elif EULER_order_out==313:
            psi=np.arctan2((Q[0]*Q[2]+Q[1]*Q[3]),(Q[0]*Q[3]-Q[1]*Q[2]))
            theta=np.arccos(Q[3]**2-Q[0]**2-Q[1]**2+Q[2]**2)
            phi=np.arctan2((Q[0]*Q[2]-Q[1]*Q[3]),(Q[0]*Q[3]+Q[1]*Q[2]))
            Euler_type=2
        elif EULER_order_out==131:
            psi=np.arctan2((Q[0]*Q[2]-Q[1]*Q[3]),(Q[0]*Q[1]+Q[2]*Q[3]))
            theta=np.arccos(Q[3]**2+Q[0]**2-Q[1]**2-Q[2]**2)
            phi=np.arctan2((Q[0]*Q[2]+Q[1]*Q[3]),(Q[2]*Q[3]-Q[0]*Q[1]))
            Euler_type=2
        elif EULER_order_out==212:
            psi=np.arctan2((Q[0]*Q[1]-Q[2]*Q[3]),(Q[0]*Q[3]+Q[1]*Q[2]))
            theta=np.arccos(Q[3]**2-Q[0]**2+Q[1]**2-Q[2]**2)
            phi=np.arctan2((Q[0]*Q[1]+Q[2]*Q[3]),(Q[0]*Q[3]-Q[1]*Q[2]))
            Euler_type=2
        elif EULER_order_out==323:
            psi=np.arctan2((Q[1]*Q[2]-Q[0]*Q[3]),(Q[0]*Q[2]+Q[1]*Q[3]))
            theta=np.arccos(Q[3]**2-Q[0]**2-Q[1]**2+Q[2]**2)
            phi=np.arctan2((Q[0]*Q[3]+Q[1]*Q[2]),(Q[1]*Q[3]-Q[0]*Q[2]))
            Euler_type=2
        elif EULER_order_out==123:
            psi=np.arctan2(2*(Q[0]*Q[3]-Q[1]*Q[2]),(Q[3]**2-Q[0]**2-Q[1]**2+Q[2]**2))
            theta=np.arcsin(2*(Q[0]*Q[2]+Q[1]*Q[3]))
            phi=np.arctan2(2*(Q[2]*Q[3]-Q[0]*Q[1]),(Q[3]**2+Q[0]**2-Q[1]**2-Q[2]**2))
            Euler_type=1
        elif EULER_order_out==231:
            psi=np.arctan2(2*(Q[1]*Q[3]-Q[0]*Q[2]),(Q[3]**2+Q[0]**2-Q[1]**2-Q[2]**2))
            theta=np.arcsin(2*(Q[0]*Q[1]+Q[2]*Q[3]))
            phi=np.arctan2(2*(Q[0]*Q[3]-Q[2]*Q[1]),(Q[3]**2-Q[0]**2+Q[1]**2-Q[2]**2))
            Euler_type=1
        elif EULER_order_out==312:
            psi=np.arctan2(2*(Q[2]*Q[3]-Q[0]*Q[1]),(Q[3]**2-Q[0]**2+Q[1]**2-Q[2]**2))
            theta=np.arcsin(2*(Q[0]*Q[3]+Q[1]*Q[2]))
            phi=np.arctan2(2*(Q[1]*Q[3]-Q[2]*Q[0]),(Q[3]**2-Q[0]**2-Q[1]**2+Q[2]**2))
            Euler_type=1
        elif EULER_order_out==132:
            psi=np.arctan2(2*(Q[0]*Q[3]+Q[1]*Q[2]),(Q[3]**2-Q[0]**2+Q[1]**2-Q[2]**2))
            theta=np.arcsin(2*(Q[2]*Q[3]-Q[0]*Q[1]))
            phi=np.arctan2(2*(Q[0]*Q[2]+Q[1]*Q[3]),(Q[3]**2+Q[0]**2-Q[1]**2-Q[2]**2))
            Euler_type=1
        elif EULER_order_out==213:
            psi=np.arctan2(2*(Q[0]*Q[2]+Q[1]*Q[3]),(Q[3]**2-Q[0]**2-Q[1]**2+Q[2]**2))
            theta=np.arcsin(2*(Q[0]*Q[3]-Q[1]*Q[2]))
            phi=np.arctan2(2*(Q[0]*Q[1]+Q[2]*Q[3]),(Q[3]**2-Q[0]**2+Q[1]**2-Q[2]**2))
            Euler_type=1
        elif EULER_order_out==321:
            psi=np.arctan2(2*(Q[0]*Q[1]+Q[2]*Q[3]),(Q[3]**2+Q[0]**2-Q[1]**2-Q[2]**2))
            theta=np.arcsin(2*(Q[1]*Q[3]-Q[0]*Q[2]))
            phi=np.arctan2(2*(Q[0]*Q[3]+Q[2]*Q[1]),(Q[3]**2-Q[0]**2-Q[1]**2+Q[2]**2))
            Euler_type=1
        else:
            print ('Exception: Invalid output Euler angle order type (conversion string).')           
        
        if(np.isreal([psi,theta,phi])).any()==0:
            print ('Exception: Unreal Euler output.  Input resides too close to np.singularity.  Please choose different output type.')
        
        if units=='deg':
            OUTPUT=np.mod(np.asarray([psi,theta,phi])*180/np.pi,360)  # deg
        else:
            OUTPUT=np.mod(np.asarray([psi,theta,phi]),2*np.pi)  # rad
        if Euler_type==1:
            sing_chk=np.abs(theta)>(np.pi/2-0.0017) #89.9 deg
            #sing_chk=sing_chk[sing_chk>0]
            if sing_chk:
                print ('Exception: Input rotation %s s resides too close to'\
                          'Type 1 Euler singularity.\nType 1 '\
                          'Euler singularity occurs when second '\
                          'angle is -90 or 90 degrees.\nPlease choose '\
                          'different output type.' % str(sing_chk))
            
        elif Euler_type==2:
            # TODO 
            print "Euler Type 2 Singularity Check not implemented"
            #sing_chk=[find(np.abs(theta*180/np.pi)<0.1)find(np.abs(theta*180/np.pi-180)<0.1)find(np.abs(theta*180/np.pi-360))<0.1]
            sing_chk=sort(sing_chk(sing_chk>0))
            if size(sing_chk,1)>=1:
                print ('Exception: Input rotation ## s resides too close to Type 2 Euler np.singularity.\nType 2 Euler np.singularity occurs when second angle is 0 or 180 degrees.\nPlease choose different output type.',str(sing_chk(1,1)))
            
    return OUTPUT


def get_pose(transform,seq):
    """
    this fuction provides a different interface to the SpinCalc function.
    
    It allows you to pass a transform (4x4 numpy transfromation matrix) and 
    it will return a pose vector (Tx,Ty,Tz,RotX,RotY,RotZ).  Rotations will 
    be in radians.  
    seq is the euler sequence requested (eg: '123' or '213')
    
    """
    trans = transform[0:3,3]
    angles = SpinCalc('DCMtoEA'+seq,transform[0:3,0:3])
    pose = [trans[0],trans[1],trans[2],angles[0],angles[1],angles[2]]
    return np.asarray(pose)

if __name__ == '__main__':
    ea_in = [90,0,90]
    dcm_in = np.asarray([[  2.22044605e-16,   2.22044605e-16,   1.00000000e+00],
                         [ -1.00000000e+00,   0.00000000e+00,   2.22044605e-16],
                         [  0.00000000e+00,  -1.00000000e+00,   2.22044605e-16]])
    dcm_out = SpinCalc('EA123toDCM',ea_in)
    print "EA123toDCM \n\t%s->DCM\n\t%s" % (str(ea_in),str(dcm_out))
    ea_out = SpinCalc('DCMtoEA123',dcm_out)
    print "DCMtoEA123 \n\tDCM->%s" % str(ea_out)
    ea_out2 = SpinCalc('EA123toEA321',ea_in)
    print "EA123toEA321 \n\t%s->%s" % (str(ea_in),str(ea_out2))
    q = np.asarray([ 0.18257418583506, 0.36514837167011, 0.54772255750517, 0.73029674334022 ])
    #q=q/(np.sqrt((q**2).sum()))
    ea_out3 = SpinCalc('QtoEA213',q)
    print "QtoEA213 \n\t%s->%s" % (str(q),str(ea_out3))
    q_out = SpinCalc('EA213toQ',ea_out3)
    print "EA213toQ \n\t%s->%s" % (str(ea_out3),str(q_out))
    dcm_out2 = SpinCalc('QtoDCM',q)
    print "QtoDCM \n\t%s->DCM\n\t%s" % (str(q),str(dcm_out2))
    q_out2 = SpinCalc('DCMtoQ',dcm_out2)
    print "DCMtoQ \n\tDCM->%s" % (str(q_out2))
    print "Singular Test: ", SpinCalc('EA123toEA321',[0,90,0])
    print "(60, 70, 80)->DCM: ", SpinCalc('EA312toDCM',[60,70,80])
    print "(60, 70, 80)->Q: ", SpinCalc('EA312toQ',[60,70,80])
    print "(60, 70, 80)->EV: ", (SpinCalc('EA312toEV',[60,70,80]))*np.asarray([1,1,1,np.pi/180.0])
    print "(10,12.5,14.5)->EA132: ", SpinCalc('EA321toEA132',[10,12.5,14.5])
    print "EA123(10,12.5,14.5)->DCM: ", SpinCalc('EA321toDCM',[10,12.5,14.5])
    print "EA321(10,12.5,14.5)->DCM: ", SpinCalc('EA321toDCM',[10,12.5,14.5])
    print "EA123(10,12.5,14.5)->EA132: ", SpinCalc('EA321toEA132',[10,12.5,14.5])
    print "(120, 70, 80)->EV: ", (SpinCalc('EA312toEV',[120,70,80]))*np.asarray([1,1,1,np.pi/180.0])